#include "../Tools/GLHeaders.hpp" 
#include "../Tools/Bone.hpp"
#include "../Tools/Model.hpp"
#include "../Tools/Vector.hpp"
#include "../Tools/Chrono.hpp"
#include <cmath>

#include "Robot.hpp"
#include "ModelHead.hpp"
#include "ModelNeck.hpp"
#include "ModelTrunk.hpp"
#include "ModelPelvis.hpp"
#include "ModelLeftArm.hpp"
#include "ModelRightArm.hpp"
#include "ModelLeftForearm.hpp"
#include "ModelRightForearm.hpp"
#include "ModelLeftHand.hpp"
#include "ModelRightHand.hpp"
#include "ModelLeftThigh.hpp"
#include "ModelRightThigh.hpp"
#include "ModelLeftLeg.hpp"
#include "ModelRightLeg.hpp"
#include "ModelLeftFoot.hpp"
#include "ModelRightFoot.hpp"

Robot::Robot(void){
	//Initialise Bone
	time = new Chrono;

  b_head = new Bone(0);
	b_neck = new Bone(1);
	b_trunk = new Bone(3);
	b_pelvis = new Bone(3);
	b_leftArm = new Bone(1);
	b_rightArm = new Bone(1);
	b_leftForearm = new Bone(1);
	b_rightForearm = new Bone(1);
	b_leftHand = new Bone(0);
	b_rightHand = new Bone(0);
	b_leftThigh = new Bone(1);
	b_rightThigh = new Bone(1);
	b_leftLeg = new Bone(1);
	b_rightLeg = new Bone(1);
	b_leftFoot = new Bone(0);
	b_rightFoot = new Bone(0);
	
  m_head = new ModelHead;
  m_neck = new ModelNeck;
  m_trunk = new ModelTrunk;
  m_pelvis = new ModelPelvis;
  m_leftArm = new ModelLeftArm;
  m_rightArm = new ModelRightArm;
  m_leftForearm = new ModelLeftForearm;
  m_rightForearm = new ModelRightForearm;
  m_leftHand = new ModelLeftHand;
  m_rightHand = new ModelRightHand;
  m_leftThigh = new ModelLeftThigh;
  m_rightThigh = new ModelRightThigh;
  m_leftLeg = new ModelLeftLeg;
  m_rightLeg = new ModelRightLeg;
  m_leftFoot = new ModelLeftFoot;
  m_rightFoot = new ModelRightFoot;

	//Initialise Model for each bones
  b_head->SetModel(m_head);
	b_neck->SetModel(m_neck);
	b_trunk->SetModel(m_trunk);
	b_pelvis->SetModel(m_pelvis);
	b_leftArm->SetModel(m_leftArm);
	b_rightArm->SetModel(m_rightArm);
	b_leftForearm->SetModel(m_leftForearm);
	b_rightForearm->SetModel(m_rightForearm);
	b_leftHand->SetModel(m_leftHand);
	b_rightHand->SetModel(m_rightHand);
	b_leftThigh->SetModel(m_leftThigh);
	b_rightThigh->SetModel(m_rightThigh);
	b_leftLeg->SetModel(m_leftLeg);
	b_rightLeg->SetModel(m_rightLeg);
	b_leftFoot->SetModel(m_leftFoot);
	b_rightFoot->SetModel(m_rightFoot);

  //Initialise Connection between bones
  b_neck->SetConnect(0, b_head);
  b_head->SetTranslation(0.0f, 1.5f, 0.0f);
  b_head->SetRotation(1.0f, 0.0f, 0.0f);

  b_trunk->SetConnect(0, b_neck);
  b_neck->SetTranslation(0.0f, 10.0f, 0.0f);
  b_neck->SetRotation(1.0f, 0.0f, 0.0f);
  b_trunk->SetConnect(1, b_leftArm);
  b_leftArm->SetTranslation(4.0f, 8.75f, 0.0f);
  b_leftArm->SetRotation(1.0f, 0.0f, 0.0f); 
  b_trunk->SetConnect(2, b_rightArm);
  b_rightArm->SetTranslation(-4.0f, 8.75f, 0.0f);
  b_rightArm->SetRotation(-1.0f, 0.0f, 0.0f); 

  b_pelvis->SetConnect(0, b_trunk);
  b_trunk->SetTranslation(0.0f, 0.0f, 0.0f);
  b_trunk->SetRotation(1.0f, 0.0f, 0.0f);
  b_pelvis->SetConnect(1, b_leftThigh);
  b_leftThigh->SetTranslation(3.5f,-3.0f, 0.0f);
  b_leftThigh->SetRotation(1.0f, 0.0f, 0.0f);
  b_pelvis->SetConnect(2, b_rightThigh);
  b_rightThigh->SetTranslation(-3.5f,-3.0f, 0.0f);
  b_rightThigh->SetRotation(-1.0f, 0.0f, 0.0f);

  b_leftArm->SetConnect(0, b_leftForearm);
  b_leftForearm->SetTranslation(2.5f,-5.5f, 0.0f);
  b_leftForearm->SetRotation(1.0f, 0.0f, 0.0f);

  b_rightArm->SetConnect(0, b_rightForearm);
  b_rightForearm->SetTranslation(-2.5f,-5.5f, 0.0f);
  b_rightForearm->SetRotation(-1.0f, 0.0f, 0.0f);

  b_leftForearm->SetConnect(0, b_leftHand);
  b_leftHand->SetTranslation(0.0f,-5.5f, 0.0f);
  b_leftHand->SetRotation(0.0f, 1.0f, 0.0f); 

  b_rightForearm->SetConnect(0, b_rightHand);
  b_rightHand->SetTranslation(0.0f,-5.5f, 0.0f);
  b_rightHand->SetRotation(0.0f, 1.0f, 0.0f);

  b_leftThigh->SetConnect(0, b_leftLeg);
  b_leftLeg->SetTranslation(0.0f,-7.0f, 0.0f);
  b_leftLeg->SetRotation(1.0f, 0.0f, 0.0f);

  b_rightThigh->SetConnect(0, b_rightLeg);
  b_rightLeg->SetTranslation(0.0f,-7.0f, 0.0f);
  b_rightLeg->SetRotation(-1.0f, 0.0f, 0.0f);

  b_leftLeg->SetConnect(0, b_leftFoot);
  b_leftFoot->SetTranslation(0.0f,-7.0f, 0.0f);
  b_leftFoot->SetRotation(1.0f, 0.0f, 0.0f);

  b_rightLeg->SetConnect(0, b_rightFoot);
  b_rightFoot->SetTranslation(0.0f,-7.0f, 0.0f);
  b_rightFoot->SetRotation(-1.0f, 0.0f, 0.0f);

  step = 0;
  speed = 1.0f;
	activate = false;
  time->Reset();
  time->Activate();
}

Robot::~Robot(void){
	delete time;

  delete b_head ;
  delete b_neck ;
  delete b_trunk;
  delete b_pelvis;
  delete b_leftArm;
  delete b_rightArm;
  delete b_leftForearm;
  delete b_rightForearm;
  delete b_leftHand;
  delete b_rightHand;
  delete b_leftThigh;
  delete b_rightThigh;
  delete b_leftLeg;
  delete b_rightLeg;
  delete b_leftFoot;
  delete b_rightFoot;

  delete m_head ;
  delete m_neck ;
  delete m_trunk;
  delete m_pelvis;
  delete m_leftArm;
  delete m_rightArm;
  delete m_leftForearm;
  delete m_rightForearm;
  delete m_leftHand;
  delete m_rightHand;
  delete m_leftThigh;
  delete m_rightThigh;
  delete m_leftLeg;
  delete m_rightLeg;
  delete m_leftFoot;
  delete m_rightFoot;
}

void Robot::Draw(void){
  GLfloat tmp;

	if(activate){
		time->Activate();
	}
	else{
		time->Desactivate();
	}

  glPushMatrix();
	glScalef(1.0f/31.5f, 1.0f/31.5f, 1.0f/31.5f);
	glTranslatef(0.0f, 18.0f, 0.0f);
	// Walking
  tmp = time->GetTime();
  if(step == 0){     
    b_leftArm->SetAngle(-15.0f);
    b_leftForearm->SetAngle(-20.0f);
    b_rightArm->SetAngle(-15.0f);
    b_rightForearm->SetAngle(20.0f);

    b_leftThigh->SetAngle(-20.0f);
    b_leftLeg->SetAngle(20.0f);
    step++;
  }
  else {
  }

	b_pelvis->Draw();
	glPopMatrix();
}

void Robot::Pause(void){
  time->Desactivate();
}

void Robot::Continue(void){
  time->Activate();
}

void Robot::Desactivate(void){
	activate = false;
}

void Robot::Activate(void){
	activate = true;
}
